copp\copp\copp3\opt3/
topp3_socp.rs

1//! 3rd-order Time-Optimal Path Parameterization (TOPP3) based on second-order cone programming (SOCP).
2//!
3//! # Method identity
4//! This module implements the **optimization backend** for TOPP3-QP by transforming
5//! third-order path-parameterization constraints/objective into Clarabel-compatible
6//! conic form and solving with SOCP.
7//!
8//! # Discrete variables (local notation)
9//! On a path grid `s[0..=n]`:
10//! - `a[k]` denotes $\dot{s}_k^2$;
11//! - `b[k]` denotes $\ddot{s}_k$;
12//! - auxiliary variables `xi[k]` and `eta[k]` satisfy reciprocal-SOC coupling for
13//!   the time objective in QP form.
14//! - decision vector is organized as
15//!   `x = [a[0..=n], b[0..=n], xi[0..len_xi), eta[0..len_xi)]`.
16//!
17//! # High-level pipeline
18//! 1. Validate boundary/index contracts.
19//! 2. Assemble standard TOPP3 conic constraints.
20//! 3. Add QP-specific SOC constraints for `(xi, eta)` and reciprocal coupling.
21//! 4. Build sparse matrices `A`, `P`, vector `q`, and solve by Clarabel.
22//! 5. Apply status acceptance policy ([`ClarabelOptions::is_allow`](crate::solver::copp2_socp::ClarabelOptions::is_allow)) and extract
23//!    a [`Topp3Profile`](crate::solver::topp3_socp::Topp3Profile) only when accepted.
24//!
25//! # API layering
26//! - [`topp3_socp`](crate::solver::topp3_socp::topp3_socp): strict/normal API, returns only accepted [`Topp3Profile`](crate::solver::topp3_socp::Topp3Profile).
27//! - [`topp3_socp_expert`](crate::solver::topp3_socp::topp3_socp_expert): expert API returning `(Option<Topp3Profile>, DefaultSolution<f64>)`.
28//! - [`topp3_socp_expert_with_info`](crate::solver::topp3_socp::topp3_socp_expert_with_info): expert API plus Clarabel linear-solver
29//!   metadata for wrappers that need solver-side diagnostics.
30
31use crate::copp::clarabel_backend::ConstraintsClarabel;
32use crate::copp::copp3::Topp3Profile;
33use crate::copp::copp3::formulation::{Topp3Problem, get_weight_a_topp3};
34use crate::copp::copp3::opt3::ClarabelExpertInfor3rd;
35use crate::copp::copp3::opt3::clarabel_constraints::{
36    clarabel_standard_capacity_topp3, clarabel_standard_constraint_topp3,
37};
38use crate::copp::{ClarabelOptions, clarabel_to_copp3_solution};
39use crate::diag::{
40    CoppError, DebugVerboser, SilentVerboser, SummaryVerboser, TraceVerboser, Verboser, Verbosity,
41    check_boundary_state_copp3_valid, check_s_interval_valid, format_duration_human,
42};
43use clarabel::algebra::CscMatrix;
44use clarabel::solver::SupportedConeT::{NonnegativeConeT, SecondOrderConeT};
45use clarabel::solver::{DefaultSolution, DefaultSolver, IPSolver, SupportedConeT};
46
47/// Strict TOPP3-SOCP API for production use.
48///
49/// # Purpose
50/// Use this entry when caller only needs a valid [`Topp3Profile`](crate::solver::topp3_socp::Topp3Profile) and treats
51/// non-accepted solver statuses as hard failures.
52///
53/// # Contract
54/// - Internally calls [`topp3_socp_expert`](crate::solver::topp3_socp::topp3_socp_expert).
55/// - Returns `Ok(Topp3Profile { .. })` **iff** `options.is_allow(solution.status)` is `true`.
56/// - Returns `Err(CoppError::ClarabelSolverStatus(...))` when status is not accepted.
57///
58/// # Returns
59/// Returns accepted TOPP3 profile.
60///
61/// # Errors
62/// Returns [`CoppError`](crate::diag::CoppError) on conic-model/solver failures and non-accepted solver status.
63///
64/// More details are provided in the documentation of [`topp3_socp_expert`](crate::solver::topp3_socp::topp3_socp_expert).
65pub fn topp3_socp(
66    problem: &Topp3Problem,
67    options: &ClarabelOptions,
68) -> Result<Topp3Profile, CoppError> {
69    let (result, solution) = topp3_socp_expert(problem, options)?;
70    result.ok_or_else(|| CoppError::ClarabelSolverStatus("topp3_socp".into(), solution.status))
71}
72
73/// Expert TOPP3-SOCP API with full Clarabel solution exposure.
74///
75/// # Return contract
76/// - `Ok((Some(result), solution))`: status accepted by `options.is_allow(solution.status)`.
77/// - `Ok((None, solution))`: solve finished but status not accepted.
78/// - `Err(...)`: input/model/solver-construction runtime failures.
79///
80/// # Returns
81/// Returns tuple `(Option<Topp3Profile>, DefaultSolution<f64>)` for diagnostic pipelines.
82///
83/// # Errors
84/// Returns [`CoppError`](crate::diag::CoppError) only for true build/runtime failures.
85///
86/// # Contract
87/// - caller handles `None` profile when status is not accepted;
88/// - acceptance policy is controlled by `options.is_allow`.
89///
90/// # Verbosity behavior
91/// Logging is layered by `options.verbosity()`:
92/// - [`Silent`](Verbosity::Silent): no algorithm logs;
93/// - [`Summary`](Verbosity::Summary): lifecycle milestones and elapsed time;
94/// - [`Debug`](Verbosity::Debug): assembly-level counters and stage summaries;
95/// - [`Trace`](Verbosity::Trace): fine-grained stage deltas and solver snapshot diagnostics.
96pub fn topp3_socp_expert(
97    problem: &Topp3Problem,
98    options: &ClarabelOptions,
99) -> Result<(Option<Topp3Profile>, DefaultSolution<f64>), CoppError> {
100    let info = topp3_socp_expert_with_info(problem, options)?;
101    let _ = &info.linsolver;
102    Ok((info.result, info.solution))
103}
104
105/// Expert TOPP3-SOCP API with Clarabel solution and linear-solver diagnostics.
106///
107/// Use this variant when callers need more than
108/// [`DefaultSolution`](clarabel::solver::DefaultSolution), because Clarabel stores linear-solver metadata on the
109/// solver `info` object rather than inside the returned solution.
110pub fn topp3_socp_expert_with_info(
111    problem: &Topp3Problem,
112    options: &ClarabelOptions,
113) -> Result<ClarabelExpertInfor3rd, CoppError> {
114    match options.verbosity() {
115        Verbosity::Silent => topp3_socp_core(problem, (options, SilentVerboser)),
116        Verbosity::Summary => topp3_socp_core(problem, (options, SummaryVerboser::new())),
117        Verbosity::Debug => topp3_socp_core(problem, (options, DebugVerboser::new())),
118        Verbosity::Trace => topp3_socp_core(problem, (options, TraceVerboser::new())),
119    }
120}
121
122/// Core implementation for TOPP3-SOCP expert flow.
123///
124/// # Internal contract
125/// `options_verboser` packs:
126/// - `options`: acceptance policy and Clarabel numerical settings;
127/// - `verboser`: concrete logger implementation chosen by external verbosity dispatch.
128///
129/// # Invariants
130/// - decision-variable layout always starts with contiguous `a[0..=n]` and `b[0..=n]`;
131/// - auxiliary block `[xi, eta]` has shared length `length_xi_eta(n, num_stationary)`;
132/// - extracted `(a,b)` is produced only through [`clarabel_to_copp3_solution`](crate::solver::copp3_socp::clarabel_to_copp3_solution) when status is accepted.
133fn topp3_socp_core(
134    problem: &Topp3Problem,
135    options_verboser: (&ClarabelOptions, impl Verboser),
136) -> Result<ClarabelExpertInfor3rd, CoppError> {
137    let (options, mut verboser) = options_verboser;
138    let idx_s_start = problem.idx_s_start;
139    let a_boundary = problem.a_boundary;
140    let b_boundary = problem.b_boundary;
141    let num_stationary = problem.num_stationary;
142    if verboser.is_enabled(Verbosity::Summary) {
143        verboser.record_start_time();
144    }
145    if verboser.is_enabled(Verbosity::Trace) {
146        let settings = options.clarabel_settings();
147        crate::verbosity_log!(
148            crate::diag::Verbosity::Summary,
149            "topp3_socp: options snapshot -> allow(almost={}, max_iter={}, max_time={}, callback_term={}, insufficient_progress={}), tol_gap_rel={}, tol_feas={}, max_iter={}, verbose={}",
150            options.is_allow(clarabel::solver::SolverStatus::AlmostSolved),
151            options.is_allow(clarabel::solver::SolverStatus::MaxIterations),
152            options.is_allow(clarabel::solver::SolverStatus::MaxTime),
153            options.is_allow(clarabel::solver::SolverStatus::CallbackTerminated),
154            options.is_allow(clarabel::solver::SolverStatus::InsufficientProgress),
155            settings.tol_gap_rel,
156            settings.tol_feas,
157            settings.max_iter,
158            settings.verbose
159        );
160    }
161
162    // Check input validity
163    check_boundary_state_copp3_valid(a_boundary, b_boundary)?;
164    let n = problem.a_linearization.len() - 1;
165    let idx_s_final = idx_s_start + n;
166    if verboser.is_enabled(Verbosity::Summary) {
167        crate::verbosity_log!(
168            crate::diag::Verbosity::Summary,
169            "\ntopp3_socp started: {} <= idx_s <= {}, s_len = {}, num_stationary={:?}.",
170            idx_s_start,
171            idx_s_final,
172            problem.a_linearization.len(),
173            num_stationary
174        );
175    }
176    check_s_interval_valid("topp3_socp", idx_s_start, idx_s_final)?;
177    let len_xi = length_xi_eta(n, num_stationary);
178    let id_xi_start = 2 * (n + 1);
179    let id_eta_start = id_xi_start + len_xi;
180    // Let x = [a[0,1,...,n],
181    //          b[0,1,...,n],
182    //          xi[0,1,...,len_xi-1],
183    //          eta[0,1,...,len_xi-1]]
184    //       \in R^{2*(n+1)+2*len_xi}.
185    // Step 1. Deal with constraints
186    // s=b-A*x \in cone, where A[row[i],col[i]]=val[i], A \in R^{m*(n+1)}, b \in R^m, s \in R^m
187    // -s=-b+A*x
188    // Step 1.1 create constraints
189    let (cap_val_lp, cap_b_lp, cap_cone_lp) =
190        clarabel_standard_capacity_topp3(problem.constraints, (idx_s_start, idx_s_final));
191    let (cap_val_qp, cap_b_qp, cap_cone_qp) = clarabel_capacity_topp3_qp(n);
192    if verboser.is_enabled(Verbosity::Debug) {
193        crate::verbosity_log!(
194            crate::diag::Verbosity::Summary,
195            "topp3_socp: capacity estimate lp(val={cap_val_lp}, b={cap_b_lp}, cone={cap_cone_lp}), qp(val={cap_val_qp}, b={cap_b_qp}, cone={cap_cone_qp}), n_var={}",
196            id_eta_start + len_xi
197        );
198    }
199    let mut cones = Vec::<SupportedConeT<f64>>::with_capacity(cap_cone_lp + cap_cone_qp);
200    let mut row = Vec::<usize>::with_capacity(cap_val_lp + cap_val_qp);
201    let mut col = Vec::<usize>::with_capacity(cap_val_lp + cap_val_qp);
202    let mut val = Vec::<f64>::with_capacity(cap_val_lp + cap_val_qp);
203    let mut b = Vec::<f64>::with_capacity(cap_b_lp + cap_b_qp);
204    if verboser.is_enabled(Verbosity::Trace) {
205        crate::verbosity_log!(
206            crate::diag::Verbosity::Summary,
207            "topp3_socp: allocated capacities row/col/val/b/cones <= {}/{}/{}/{}/{}",
208            cap_val_lp + cap_val_qp,
209            cap_val_lp + cap_val_qp,
210            cap_val_lp + cap_val_qp,
211            cap_b_lp + cap_b_qp,
212            cap_cone_lp + cap_cone_qp
213        );
214    }
215
216    // Step 1.2 deal with standard constraints
217    let s = problem.constraints.s_vec(idx_s_start, idx_s_final + 1)?;
218    let row_before_std = row.len();
219    let col_before_std = col.len();
220    let val_before_std = val.len();
221    let b_before_std = b.len();
222    let cones_before_std = cones.len();
223    clarabel_standard_constraint_topp3(
224        problem,
225        &s,
226        (&mut row, &mut col, &mut val, &mut b, &mut cones),
227        num_stationary,
228        &verboser,
229    )?;
230    if verboser.is_enabled(Verbosity::Trace) {
231        crate::verbosity_log!(
232            crate::diag::Verbosity::Summary,
233            "topp3_socp: standard-constraints delta row/col/val/b/cones = +{}/+{}/+{}/+{}/+{}",
234            row.len() - row_before_std,
235            col.len() - col_before_std,
236            val.len() - val_before_std,
237            b.len() - b_before_std,
238            cones.len() - cones_before_std
239        );
240    }
241    // Step 1.3 deal with additional constraints for QP
242    let row_before_qp = row.len();
243    let col_before_qp = col.len();
244    let val_before_qp = val.len();
245    let b_before_qp = b.len();
246    let cones_before_qp = cones.len();
247    clarabel_constraint_topp3_qp(
248        (&mut row, &mut col, &mut val, &mut b, &mut cones),
249        (idx_s_start, idx_s_final),
250        num_stationary,
251        id_xi_start,
252        id_eta_start,
253    );
254    if verboser.is_enabled(Verbosity::Trace) {
255        crate::verbosity_log!(
256            crate::diag::Verbosity::Summary,
257            "topp3_socp: qp-aux delta row/col/val/b/cones = +{}/+{}/+{}/+{}/+{}",
258            row.len() - row_before_qp,
259            col.len() - col_before_qp,
260            val.len() - val_before_qp,
261            b.len() - b_before_qp,
262            cones.len() - cones_before_qp
263        );
264    }
265
266    // Step 1.4 build the constraints
267    let n_var = id_eta_start + len_xi;
268    let row_len = row.len();
269    let col_len = col.len();
270    let val_len = val.len();
271    let b_len = b.len();
272    let cones_len = cones.len();
273    let a_csc = CscMatrix::new_from_triplets(b.len(), n_var, row, col, val);
274    // Step 2. objective function (time QP surrogate): min \sum w[k] * eta[k]
275    let p_object = CscMatrix::<f64>::zeros((n_var, n_var));
276    let q_object = clarabel_q_object_topp3_qp(&s, num_stationary, n_var, id_eta_start);
277    if verboser.is_enabled(Verbosity::Trace) {
278        let (q_min, q_max) = q_object
279            .iter()
280            .fold((f64::INFINITY, f64::NEG_INFINITY), |(mn, mx), &v| {
281                (mn.min(v), mx.max(v))
282            });
283        crate::verbosity_log!(
284            crate::diag::Verbosity::Summary,
285            "topp3_socp: matrix built with m={}, n={}, A.nnz={}, P.nnz={}, q_range=[{}, {}]",
286            b_len,
287            n_var,
288            a_csc.nnz(),
289            p_object.nnz(),
290            q_min,
291            q_max
292        );
293    }
294    if verboser.is_enabled(Verbosity::Summary) {
295        crate::verbosity_log!(
296            crate::diag::Verbosity::Summary,
297            "topp3_socp: ready to solve with row/col/val/b/cones = {row_len}/{col_len}/{val_len}/{b_len}/{cones_len} and n_var = {n_var}.",
298        );
299    }
300    // Step 3. solve the SOCP problem
301    let settings = options.clarabel_settings().clone();
302    let mut solver = DefaultSolver::<f64>::new(&p_object, &q_object, &a_csc, &b, &cones, settings)
303        .map_err(|e| CoppError::ClarabelSolverError("topp3_socp".into(), e))?;
304    solver.solve();
305    let linsolver = solver.info.linsolver.clone();
306    let solution = solver.solution;
307    if verboser.is_enabled(Verbosity::Summary) {
308        crate::verbosity_log!(
309            crate::diag::Verbosity::Summary,
310            "topp3_socp: solve done, status = {:?}, elapsed = {}.",
311            solution.status,
312            format_duration_human(verboser.elapsed())
313        );
314    }
315    if verboser.is_enabled(Verbosity::Trace) {
316        let show = solution.x.len().min(3);
317        crate::verbosity_log!(
318            crate::diag::Verbosity::Summary,
319            "topp3_socp: solution x_len={}, head={:?}",
320            solution.x.len(),
321            &solution.x[0..show]
322        );
323    }
324    let result = if options.is_allow(solution.status) {
325        Some(clarabel_to_copp3_solution(
326            &solution.x.as_slice()[0..2 * (n + 1)],
327            &s,
328            num_stationary,
329        ))
330    } else {
331        None
332    };
333    if verboser.is_enabled(Verbosity::Trace) {
334        crate::verbosity_log!(
335            crate::diag::Verbosity::Summary,
336            "topp3_socp: allow(status)={}, extracted_profile={}",
337            options.is_allow(solution.status),
338            if result.is_some() {
339                "Some(Topp3Profile)"
340            } else {
341                "None"
342            }
343        );
344    }
345    Ok(ClarabelExpertInfor3rd {
346        result,
347        solution,
348        linsolver,
349    })
350}
351
352/// Determine the length of `xi` and `eta` in the decision variable `x`.
353#[inline(always)]
354fn length_xi_eta(n: usize, num_stationary: (usize, usize)) -> usize {
355    n + 1 - num_stationary.0.max(1) - num_stationary.1.max(1)
356}
357
358/// Return `k_skip`, where `eta[k] = 1/sqrt(a[k + k_skip])`.
359#[inline(always)]
360fn skip_a_for_xi(num_stationary_start: usize) -> usize {
361    num_stationary_start.max(1)
362}
363
364/// Create the constraints for clarabel TOPP3-QP.
365/// `idx_s_interval`: (idx_s_start, idx_s_final), the interval of s for which we want to compute the time-optimal profile.
366/// `num_stationary`: (num_stationary_start, num_stationary_final), the number of stationary points at the start and final of the interval.
367/// `id_xi_start`: the starting index of xi in the decision variable x.
368/// `id_eta_start`: the starting index of eta in the decision variable x.
369fn clarabel_constraint_topp3_qp(
370    constraints: ConstraintsClarabel,
371    idx_s_interval: (usize, usize),
372    num_stationary: (usize, usize),
373    id_xi_start: usize,
374    id_eta_start: usize,
375) {
376    let (idx_s_start, idx_s_final) = idx_s_interval;
377    let n = idx_s_final - idx_s_start;
378    // s=b-A*x \in cone, where A[row[i],col[i]]=val[i]
379    // -s=-b+A*x
380    let (row, col, val, b, cones) = constraints;
381    // Add constraints for xi and eta
382    // xi[k] >= 0, eta[k] >= 0
383    // norm2([2, xi[k] - eta[k]]) <= xi[k] + eta[k]
384    // xi[k] * xi[k] <= a[k + k_skip]
385    let len_xi = length_xi_eta(n, num_stationary);
386    let k_skip = skip_a_for_xi(num_stationary.0);
387    // Step 1. xi[i] >= 0
388    // A*x-b = -s = -1*xi[k] <= 0
389    row.extend(b.len()..(b.len() + len_xi));
390    col.extend(id_xi_start..(id_xi_start + len_xi));
391    val.resize(val.len() + len_xi, -1.0);
392    b.resize(b.len() + len_xi, 0.0);
393    // Step 2. eta[i] >= 0
394    // A*x-b = -s = -1*eta[k] <= 0
395    row.extend(b.len()..(b.len() + len_xi));
396    col.extend(id_eta_start..(id_eta_start + len_xi));
397    val.resize(val.len() + len_xi, -1.0);
398    b.resize(b.len() + len_xi, 0.0);
399    cones.push(NonnegativeConeT(2 * len_xi));
400    // Step 3. norm2([2, xi[k] - eta[k]]) <= xi[k] + eta[k]
401    // -A*x+b = s = [xi[k] + eta[k], xi[k] - eta[k], 2] \in SOC
402    for k in 0..len_xi {
403        // xi[k] + eta[k]
404        row.resize(row.len() + 2, b.len());
405        col.extend([id_xi_start + k, id_eta_start + k]);
406        val.extend([-1.0, -1.0]);
407        b.push(0.0);
408        // xi[k] - eta[k]
409        row.resize(row.len() + 2, b.len());
410        col.extend([id_xi_start + k, id_eta_start + k]);
411        val.extend([-1.0, 1.0]);
412        b.push(0.0);
413        // 2
414        b.push(2.0);
415    }
416    // Step 4. xi[k] * xi[k] <= a[k_skip + k]
417    // norm2([2*xi[k], a[k_skip + k] - 1]) <= a[k_skip + k] + 1
418    // -A*x+b = s = [a[k_skip + k] + 1, a[k_skip + k] - 1, 2*xi[k]] \in SOC
419    for k in 0..len_xi {
420        // a[k_skip + k] + 1
421        row.push(b.len());
422        col.push(k_skip + k);
423        val.push(-1.0);
424        b.push(1.0);
425        // a[k_skip + k] - 1
426        row.push(b.len());
427        col.push(k_skip + k);
428        val.push(-1.0);
429        b.push(-1.0);
430        // 2*xi[k]
431        row.push(b.len());
432        col.push(id_xi_start + k);
433        val.push(-2.0);
434        b.push(0.0);
435    }
436    cones.resize(cones.len() + 2 * len_xi, SecondOrderConeT(3));
437}
438
439/// Build the linear objective coefficient `q` for TOPP3-QP.
440#[inline(always)]
441fn clarabel_q_object_topp3_qp(
442    s: &[f64],
443    num_stationary: (usize, usize),
444    n_var: usize,
445    id_eta_start: usize,
446) -> Vec<f64> {
447    let mut q_object = Vec::<f64>::with_capacity(n_var);
448    let weight = get_weight_a_topp3(s, num_stationary);
449    let len_eta = length_xi_eta(s.len() - 1, num_stationary);
450    let k_skip = skip_a_for_xi(num_stationary.0);
451    q_object.resize(id_eta_start, 0.0);
452    q_object.extend(weight[k_skip..(k_skip + len_eta)].iter());
453    q_object.resize(n_var, 0.0);
454    q_object
455}
456
457/// Determine Clarabel pre-allocation capacity for TOPP3-QP auxiliary constraints.
458///
459/// Returns `(capacity_val, capacity_b, capacity_cones)` as upper bounds.
460#[inline(always)]
461fn clarabel_capacity_topp3_qp(n: usize) -> (usize, usize, usize) {
462    // Step 1. xi[k] >= 0, eta[k] >= 0
463    //         (num_val==2*len_xi; num_b==2*len_xi, num_cone==1)
464    // Step 2. [2, xi[k] - eta[k], xi[k] + eta[k]] \in SOC
465    //         (num_val==4*len_xi; num_b==3*len_xi, num_cone==len_xi)
466    // Step 3. [2*xi[k], a[num_stationary.0 + k] - 1, a[num_stationary.0 + k] + 1] \in SOC
467    //         (num_val==3*len_xi; num_b==3*len_xi, num_cone==len_xi)
468    // len_xi = n + 1 - num_stationary.0 - num_stationary.1 <= n + 1
469    let len_xi_upper_bound = n + 1;
470    (
471        9 * len_xi_upper_bound,
472        8 * len_xi_upper_bound,
473        2 * len_xi_upper_bound + 1,
474    )
475}
476
477#[cfg(test)]
478mod tests {
479    use super::*;
480    use crate::copp::copp2::stable::basic::{Topp2ProblemBuilder, s_to_t_topp2};
481    use crate::copp::copp2::stable::reach_set2::{ReachSet2Options, ReachSet2OptionsBuilder};
482    use crate::copp::copp2::stable::topp2_ra::topp2_ra;
483    use crate::copp::copp3::stable::basic::{Topp3ProblemBuilder, s_to_t_topp3};
484    use crate::copp::{ClarabelOptions, ClarabelOptionsBuilder};
485    use crate::path::{add_symmetric_axial_limits_for_test, lissajous_path_for_test};
486    use crate::robot::robot_core::Robot;
487    use crate::solver::topp3_lp::topp3_lp;
488    use core::f64;
489    use nalgebra::DMatrix;
490    use std::time::Instant;
491
492    #[test]
493    fn test_topp3_lp_qp() -> Result<(), CoppError> {
494        run_test_topp3_lp_qp_repeated(1, false)
495    }
496
497    /// Conditions: release, --include-ignored, CPU = Intel(R) Core(TM) Ultra 9 285K.
498    /// AAverage (fail 1): tc_ra = 0.3294 ms, tc_lp = 257.8678 ms, tc_qp = 327.9065 ms, tf_ra = 6.1838, tf_lp = 7.1079, tf_qp = 7.1079
499    #[test]
500    #[ignore = "slow"]
501    fn test_topp3_lp_qp_robust() -> Result<(), CoppError> {
502        run_test_topp3_lp_qp_repeated(100, true)
503    }
504
505    fn run_one_topp3_lp_qp_case(
506        options_ra: &ReachSet2Options,
507        options_lp: &ClarabelOptions,
508        options_qp: &ClarabelOptions,
509    ) -> Result<(f64, f64, f64, f64, f64, f64), CoppError> {
510        let n: usize = 1000;
511        let dim = 7;
512        let mut rng = rand::rng();
513        let (_s_uniform, path, _, _) =
514            lissajous_path_for_test(dim, n, &mut rng).expect("random range is valid");
515
516        let mut robot = Robot::with_capacity(dim, n);
517        let s = DMatrix::<f64>::from_fn(1, n, |_, j| {
518            (j as f64
519                + (if 0 < j && 2 * j < n { 0.5 } else { 0.0 }
520                    + if n > j && 2 * j > n { 0.5 } else { 0.0 })
521                    * j as f64
522                    / n as f64)
523                * (1.0 / (n - 1) as f64)
524        });
525        robot
526            .with_s(&s.as_view())?
527            .with_q_from_path_3rd(&path, 0, n)?;
528        add_symmetric_axial_limits_for_test(&mut robot, 1.0, 1.0, Some(5.0))?;
529
530        let topp2_problem = Topp2ProblemBuilder::new(&robot, (0, n - 1), (0.0, 0.0)).build()?;
531        let start = Instant::now();
532        let a_ra = topp2_ra(&topp2_problem, options_ra)?;
533        let tc_ra = start.elapsed().as_secs_f64() * 1E3;
534        let (tf_ra, _) = s_to_t_topp2(s.as_slice(), &a_ra, 0.0)?;
535
536        robot.constraints.amax_substitute(&a_ra, 0)?;
537        let topp3_problem = Topp3ProblemBuilder::new(&mut robot, 0, &a_ra, (0.0, 0.0), (0.0, 0.0))
538            .with_num_stationary_max(2)
539            .build_with_linearization()?;
540
541        let start = Instant::now();
542        let profile_lp = topp3_lp(&topp3_problem, options_lp)?;
543        let tc_lp = start.elapsed().as_secs_f64() * 1E3;
544        let (tf_lp, _) = s_to_t_topp3(s.as_slice(), profile_lp.as_parts(), 0.0)?;
545
546        let start = Instant::now();
547        let profile_qp = topp3_socp(&topp3_problem, options_qp)?;
548        let tc_qp = start.elapsed().as_secs_f64() * 1E3;
549        let (tf_qp, _) = s_to_t_topp3(s.as_slice(), profile_qp.as_parts(), 0.0)?;
550
551        Ok((tc_ra, tc_lp, tc_qp, tf_ra, tf_lp, tf_qp))
552    }
553
554    fn run_test_topp3_lp_qp_repeated(n_exp: usize, flag_print_step: bool) -> Result<(), CoppError> {
555        let options_ra = ReachSet2OptionsBuilder::new()
556            .lp_feas_tol(1E-9)
557            .a_cmp_abs_tol(1E-9)
558            .a_cmp_rel_tol(1E-9)
559            .build()?;
560        let options_lp = ClarabelOptionsBuilder::new()
561            .allow_almost_solved(true)
562            .build()?;
563        let options_qp = ClarabelOptionsBuilder::new()
564            .allow_almost_solved(true)
565            .build()?;
566
567        let mut tc_sum_ra = 0.0;
568        let mut tc_sum_lp = 0.0;
569        let mut tc_sum_qp = 0.0;
570        let mut tf_sum_ra = 0.0;
571        let mut tf_sum_lp = 0.0;
572        let mut tf_sum_qp = 0.0;
573        let mut succeed = 0;
574
575        for i_exp in 0..n_exp {
576            if let Ok((tc_ra, tc_lp, tc_qp, tf_ra, tf_lp, tf_qp)) =
577                run_one_topp3_lp_qp_case(&options_ra, &options_lp, &options_qp)
578            {
579                if flag_print_step {
580                    crate::verbosity_log!(
581                        crate::diag::Verbosity::Summary,
582                        "Exp #{}: tc_ra = {:.4} ms, tc_lp = {:.4} ms, tc_qp = {:.4} ms, tf_ra = {:.4}, tf_lp = {:.4}, tf_qp = {:.4}",
583                        i_exp + 1,
584                        tc_ra,
585                        tc_lp,
586                        tc_qp,
587                        tf_ra,
588                        tf_lp,
589                        tf_qp,
590                    );
591                }
592                tc_sum_ra += tc_ra;
593                tc_sum_lp += tc_lp;
594                tc_sum_qp += tc_qp;
595                tf_sum_ra += tf_ra;
596                tf_sum_lp += tf_lp;
597                tf_sum_qp += tf_qp;
598                succeed += 1;
599            }
600        }
601
602        crate::verbosity_log!(
603            crate::diag::Verbosity::Summary,
604            "Average (fail {}): tc_ra = {:.4} ms, tc_lp = {:.4} ms, tc_qp = {:.4} ms, tf_ra = {:.4}, tf_lp = {:.4}, tf_qp = {:.4}",
605            n_exp - succeed,
606            tc_sum_ra / succeed as f64,
607            tc_sum_lp / succeed as f64,
608            tc_sum_qp / succeed as f64,
609            tf_sum_ra / succeed as f64,
610            tf_sum_lp / succeed as f64,
611            tf_sum_qp / succeed as f64,
612        );
613
614        Ok(())
615    }
616}
copp\copp\copp3\opt3/topp3_socp.rs

copp\copp\copp3\opt3/
topp3_socp.rs
